#!usr/bin/env python
# -*- coding:utf-8 _*-
"""
@Time   :   2021/6/21  上午9:59
@Author :   HaoWANG, Foshan，China
@Email  :   haowanghk@163.com
@file   :  testing_demo.py.py
@Software : Pycharm
@Description:
"""

import uservo
import serial
import time

from MotionGroup import MotionGroup
from ServoPose import Pose
import logging

from wrist_gripper import WristGripperControl

from robot_arm import ArmMotionControl

SHORT_TIME_DELAY = 0.02  # time delay para
MEDIA_TIME_DELAY = 0.10
LONG_TIME_DELAY = 0.5

VEL = {'Very_low_spped': 10, 'LOW_SPEED': 15, 'MID_SPEED': 45, 'HIGH_SPEED': 60}

# dual arm loading test init pose
DUAL_ARM_INIT_POSE = [140.0, 45.0, 155.0, 0, -4.6, -76.5, 171.0, 0, 0, 0]
# dual arm loading test target pose
DUAL_ARM_TARGET_POSE = [140.0, 45.0, 155.0, 0, -4.6, -76.5, 171.0, 0, 0, 0]

# wrist and gripper
ending_servo_idx = [0, 1, 2, 3]


# 增加关节舵机角度的vel和iterval参数动态调整接口API
def generate_pose_vtc(angle, servo_id, vel=100, iterval=300, t_acc=35, t_dec=35, power=8000, mean_dps=70.0):
    pose_tmp = {'angle': angle, 'is_mturn': False, 'interval': iterval,
                'vel': vel, 't_acc': t_acc,
                't_dec': t_dec, 'power': power,
                'mean_dps': mean_dps}
    gen_pose = Pose(POSE=pose_tmp, SERVO_ID=servo_id)
    return gen_pose


def main(motion_group, teach_mode):
    pose_sd = {'Servo_id': 0, 'angle': None, 'is_mturn': False, 'interval': None, 'velocity': 10, 't_acc': 20,
               't_dec': 20,
               'power': 5000, 'mean_dps': 50.0}

    # dual_arm
    dual_arm = ArmMotionControl(arm_name="dual_arm", dof=6, CONTROL_MODE="DAMPING", with_wrist=False, is_debug=True,
                                damping_power=1000)
    wrist_gripper = WristGripperControl(servo_channel=ending_servo_idx, is_debug=True)

    # 初始化舵机管理器类对象、串口端口地址
    dual_arm.creat_uservo_obj(SERVO_BAUDRATE=115200)
    if dual_arm.servo_buffer(is_scan_servo=True):
        dual_arm.logging_arm_info()
    dual_arm_servo = dual_arm.get_arm_servo_list()

    # dual arm and gripper init
    dual_arm.arm_state_init(force_angle_init=False)

    dual_arm.set_all_servo_mode(CONTROL_MODE="CIRCLE")
    wrist_gripper.all_ending_init(motion_group)
    time.sleep(2)

    with open('pay_loading_data.txt', 'w') as file:
        file.write("Servo_id  Angle:   deg;   Voltage:  V;  Current:   A; Power:   W;  Torque:   N.m")
    while True:
        keyInput = input('waitKey input (c or x):')
        if keyInput == 'x':
            logging.warning('Interrupt:  The waitKey input (c or x): {}'.format(keyInput))
            break
        elif keyInput == 'c':
            # dual_arm再现

            # loading up motion
            # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
            wrist_gripper.set_servo_angle(servo_idx=0, angle=DUAL_ARM_INIT_POSE[3])
            wrist_gripper.set_servo_angle(servo_idx=1, angle=DUAL_ARM_INIT_POSE[8])

            # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
            wrist_gripper.set_servo_angle(servo_idx=2, angle=DUAL_ARM_INIT_POSE[7])
            wrist_gripper.set_servo_angle(servo_idx=3, angle=DUAL_ARM_INIT_POSE[9])

            # init pose
            for SERVO_ID in dual_arm_servo:
                dual_arm.set_single_servo_pose(
                    Pose=generate_pose_vtc(angle=DUAL_ARM_INIT_POSE[SERVO_ID], vel=VEL.get('Very_LOW_SPEED'),
                                           iterval=5000,
                                           t_acc=50,
                                           t_dec=60, power=9000, mean_dps=40, servo_id=SERVO_ID))
            time.sleep(LONG_TIME_DELAY)

            # read current servo info
            servo_id = 5
            current_servo_angle_id = dual_arm.read_single_servo_angle(SERVO_ID=servo_id)
            # current_servo_angle_id = dual_arm.read_single_servo_angle(SERVO_ID=6)

            # target pose
            for SERVO_ID in dual_arm_servo:
                dual_arm.set_single_servo_pose(
                    Pose=generate_pose_vtc(angle=DUAL_ARM_TARGET_POSE[SERVO_ID], vel=VEL.get('Very_LOW_SPEED'),
                                           iterval=5000,
                                           t_acc=50,
                                           t_dec=60, power=9000, mean_dps=40, servo_id=SERVO_ID))
            # start to record  first data *********
            file.write("**************************************")
            while ( abs(current_servo_angle_id - DUAL_ARM_TARGET_POSE[servo_id]) >= 1.0):
                current_servo_angle_id = dual_arm.read_single_servo_angle(SERVO_ID=servo_id)
                voltage = dual_arm.uservo_obj.query_voltage(servo_id=servo_id)
                # 读取电流
                current = dual_arm.uservo_obj.query_current(servo_id=servo_id)
                # 读取功率
                power = dual_arm.uservo_obj.query_power(servo_id=servo_id)
                # T=9549*P/n, n = 40 dps = (20/3)r/min
                torque = power * 9549 * 3 / 20

                # 读取温度
                file.write(
                    "{} {} {:4.1f} {:4.1f} {:4.1f} {:4.1f}".format(servo_id,
                                                                   current_servo_angle_id,
                                                                   voltage,
                                                                   current,
                                                                   power,
                                                                   torque))
                time.sleep(MEDIA_TIME_DELAY)

            #############################################
            # return to init pose
            for SERVO_ID in dual_arm_servo:
                dual_arm.set_single_servo_pose(
                    Pose=generate_pose_vtc(angle=DUAL_ARM_INIT_POSE[SERVO_ID], vel=VEL.get('Very_LOW_SPEED'),
                                           iterval=5000,
                                           t_acc=50,
                                           t_dec=60, power=9000, mean_dps=40, servo_id=SERVO_ID))

            # start to record secondary data *********
            file.write("**************************************")
            while (abs(current_servo_angle_id - DUAL_ARM_INIT_POSE[servo_id]) >= 1.0):
                current_servo_angle_id = dual_arm.read_single_servo_angle(SERVO_ID=servo_id)
                voltage = dual_arm.uservo_obj.query_voltage(servo_id=servo_id)
                # 读取电流
                current = dual_arm.uservo_obj.query_current(servo_id=servo_id)
                # 读取功率
                power = dual_arm.uservo_obj.query_power(servo_id=servo_id)
                # 读取温度
                # T=9549*P/n, n = 40 dps = (20/3)r/min
                torque = power * 9549 * 3 / 20
                file.write(
                    "{} {} {:4.1f} {:4.1f} {:4.1f} {:4.1f}".format(servo_id,
                                                                   current_servo_angle_id,
                                                                   voltage,
                                                                   current,
                                                                   power,
                                                                   torque))
                time.sleep(MEDIA_TIME_DELAY)

        else:
            logging.warning('waitKey input (c or x): {}'.format(keyInput))


# main function
if __name__ == '__main__':
    # 创建一个logger_debug

    logger = logging.getLogger()
    logger.setLevel(logging.DEBUG)  # Log等级总开关-debug
    print("Logging Set Info: logger.setLevel(logging.DEBUG)  # Log等级总开关-debug")
